#include "fft.h"
#include "filter/Hanning.h"

//#pragma DATA_SECTION (sin_tab,".zyl");
//#pragma DATA_SECTION (cos_tab,".zyl");

float fWaveR[SAMPLENUMBER];
float sin_tab[SAMPLENUMBER],cos_tab[SAMPLENUMBER];

//Modification line _start-------------------------------------------------------------------------------------------------------------------------
//The Great Fast Fourier Transform From zyl, modify V0.12
//     \|/
//     [ ]
//   [     ]
//    [   ]
// |---------|
// Note that this function is applicable to FFT raw data with 1024 samples, less than the number of untested samples.
//from zyl :

void FFT(float dataR[SAMPLENUMBER]/*,float dataI[SAMPLENUMBER]*/)
{
    int x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,xx;
    int i,j,k,b,p,L;
    float TR,TI,temp;
    float dataI[SAMPLENUMBER];

    /********** Inversion sequence ************/
    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        x0=x1=x2=x3=x4=x5=x6=x7=x8=x9=0;

        x0=i&0x01; x1=(i/2)&0x01; x2=(i/4)&0x01;
        x3=(i/8)&0x01;x4=(i/16)&0x01; x5=(i/32)&0x01;
        x6=(i/64)&0x01;x7=(i/128)&0x01; x8=(i/256)&0x01; x9=(i/512)&0x01;
        xx=x0*512 + x1*256 + x2*128 + x3*64 + x4* 32 + x5*16 + x6*8 + x7*4 + x8*2 + x9;
        dataI[xx]=dataR[i];
    }
    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        dataR[i]=dataI[i]; dataI[i]=0;
    }

    /************** The Great FFT *******************/
    for ( L=1;L<=10;L++ )
    { /* for(1) */
        b=1; i=L-1;
        while ( i>0 )
        {
            b=b*2; i--;
        } /* b= 2^(L-1) */
        for ( j=0;j<=b-1;j++ ) /* for (2) */
        {
            p=1; i=10-L;
            while ( i>0 ) /* p=pow(2,10-L)*j; */
            {
                p=p*2; i--;
            }
            p=p*j;
            for ( k=j;k<1024;k=k+2*b ) /* for (3) */
            {
                TR=dataR[k]; TI=dataI[k]; temp=dataR[k+b];
                dataR[k]=dataR[k]+dataR[k+b]*cos_tab[p]+dataI[k+b]*sin_tab[p];
                dataI[k]=dataI[k]-dataR[k+b]*sin_tab[p]+dataI[k+b]*cos_tab[p];
                dataR[k+b]=TR-dataR[k+b]*cos_tab[p]-dataI[k+b]*sin_tab[p];
                dataI[k+b]=TI+temp*sin_tab[p]-dataI[k+b]*cos_tab[p];
            } /* END for (3) */
        } /* END for (2) */
    } /* END for (1) */
    for ( i=0;i<SAMPLENUMBER/2;i++ )
    {
        DATA[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);//Energy calculation
    }
} /* END FFT */
//Modification line_end----------------------------------------------------------------------------------------------------------

void fun_fft()
{
    int i;
    int f_inex=0;
    float fmax=0;
#if ENABLE_TEST
    MakeWave();
#endif

    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        fWaveR[i] = INPUT[i];
    }

#if ENABLE_HANNNING
    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        fWaveR[i] *=hann[i];
    }
#endif
//    for ( i=0;i<SAMPLENUMBER;i++ )
//    {
//        fWaveR[i]=INPUT[i];
//    }

//    FFT(fWaveR,fWaveI);
    FFT(fWaveR);
//    for ( i=0;i<SAMPLENUMBER;i++ )
//    {
//        DATA[i]=w[i];
//        if(DATA[i]>fmax)
//        {
//            fmax=DATA[i];
//            f_inex=i;
//        }
//    }
//    UARTa_SendByte()

//    for ( i=0;i<SAMPLENUMBER;i++ )
//    {
//        fWaveR[i]=INPUT[i];
//        fWaveI[i]=0.0f;
//        w[i]=0.0f;
//    }
//    FFT(fWaveR,fWaveI);
//    for ( i=0;i<SAMPLENUMBER;i++ )
//    {
//        DATA[i]=w[i];
//    }
}



void InitForFFT()
{
    int i;
    for (i = 0; i < SAMPLENUMBER; ++i)
    {
        INPUT[i]=0;
        DATA[i]=0;
    }
    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        sin_tab[i]=sin(PI*2*i/SAMPLENUMBER);
        cos_tab[i]=cos(PI*2*i/SAMPLENUMBER);
    }
    for ( i=0;i<SAMPLENUMBER;i++ )
    {
        fWaveR[i]=INPUT[i];
    }
}

void MakeWave()
{
    int i;

    float time=0;
    float delta_time=1.0/SAMPLE_RATE;
    for ( i=0;i<SAMPLENUMBER;++i)
    {
        INPUT[i]=sin(2*PI*SAMPLE_TESTRATE*time)*300;
        time+=delta_time;
//        INPUT[i]=sin(PI*2*i/SAMPLENUMBER*120)*1024;//+sin(PI*2*i/SAMPLENUMBER*1)*1024;//+rand()%10;
    }
}



